/*
 * Author  : Leo <lipf160627@163.com>
 * Created : 2025-8-27
 * Desc    : 
 */

#include "co_mcu.h"

#include <string.h>

#include "posture.h"
#include "usart.h"

// -------------------------------ESP32与STM32之间通信协议---------------------------------------- //
// 通信协议
// 增加源组件，可以支持多个组件通过CoMcu进行通信，如Asrpro、Remote-control等，应答可以根据源组件区分
// 请求帧：帧头(2) + 命令符(1) + 长度(1) + 源组件(1) + 数据(n) + 校验码(1)
// 应答帧：帧头(2) + 命令符(1) + 长度(1) + 源组件(1) + 请求帧命令符(1) + 数据(n) + 校验码(1)
// 长度  ：不包含校验字段，防止“先有鸡还是先有蛋问题”
// 校验码：采用按字节异或，XOR简单性能最好（相比字节和、CRC校验等）, 校验码 = ^帧头 + ^命令符 + ...
// 帧头: 0xAA 0x55
// 协议相信定义见项目文档
#define CO_MCU_PKT_HEADER_SIZE          2
#define CO_MCU_PKT_CMD_SIZE             1
#define CO_MCU_PKT_LENGTH_SIZE          1
#define CO_MCU_PKT_SRC_COMPONENT_SIZE   1
#define CO_MCU_PKT_CHECK_CODE_SIZE      1

#define CO_MCU_PKT_HEADER_START         0
#define CO_MCU_PKT_CMD_START            CO_MCU_PKT_HEADER_SIZE
#define CO_MCU_PKT_LENGTH_START         (CO_MCU_PKT_CMD_START + CO_MCU_PKT_CMD_SIZE)
#define CO_MCU_PKT_DATA_START           (CO_MCU_PKT_LENGTH_START + CO_MCU_PKT_LENGTH_SIZE)
#define CO_MCU_PKT_SRC_COMPONENT_START  CO_MCU_PKT_DATA_START // data 部分第一byte

#define CO_MCU_PKT_DATA_SIZE_MAX        (CO_MCU_PKT_SRC_COMPONENT_SIZE + CO_MCU_PKT_CMD_SIZE + 8)
#define CO_MCU_PKT_SIZE_MAX             (CO_MCU_PKT_DATA_START + CO_MCU_PKT_DATA_SIZE_MAX + CO_MCU_PKT_CHECK_CODE_SIZE)

#define CO_MCU_HEADER_MAGIC1            0xAA
#define CO_MCU_HEADER_MAGIC2            0x55

// 动作控制码
#define CO_MCU_CMD_INVALID              0x00
#define CO_MCU_CMD_FORWARD              0x01
#define CO_MCU_CMD_BACKWARD             0x02
#define CO_MCU_CMD_TURN_LEFT            0x03
#define CO_MCU_CMD_TURN_RIGHT           0x04
#define CO_MCU_CMD_STOP                 0x05
#define CO_MCU_CMD_SET_SPEED            0x06
// 查询码
#define CO_MCU_CMD_GET_POS              0x10
// 应答码
#define CO_MCU_CMD_RESULT_ACK           0xE0
#define CO_MCU_CMD_PROCESS_ACK          0xE1
#define CO_MCU_CMD_QUERY_ACK            0xE2
#define CO_MCU_CMD_TERMINATE            0xFE

/* UART1 Rx buffer to store packet from ESP32 */
#define CO_MCU_RX_BUF_SIZE CO_MCU_PKT_SIZE_MAX
#define CO_MCU_TX_BUF_SIZE CO_MCU_PKT_SIZE_MAX
uint8_t CoMcu_rxBuf[CO_MCU_RX_BUF_SIZE];
uint8_t CoMcu_txBuf[CO_MCU_TX_BUF_SIZE];

/* UART1 Rx semaphore */
SemaphoreHandle_t coMcu_uart1RxSemaphore;

static bool CoMcu_VerifyCheckCode(const uint8_t *recvBuf, uint8_t dataLength) {
    uint8_t checkCode = 0;
    for (uint8_t i = 0; i < (CO_MCU_PKT_DATA_START + dataLength); i++) {
        checkCode ^= CoMcu_rxBuf[i];
    }
    return (checkCode == CoMcu_rxBuf[CO_MCU_PKT_DATA_START + dataLength]);
}

static void CoMcu_CalcCheckCode(uint8_t dataLength) {
    uint8_t checkCode = 0;
    for (uint8_t i = 0; i < (CO_MCU_PKT_DATA_START + dataLength); i++) {
        checkCode ^= CoMcu_txBuf[i];
    }
    CoMcu_txBuf[CO_MCU_PKT_DATA_START + dataLength] = checkCode;
}

/* Handle packet from ESP32
 * packet = header[2] + cmd[1] + data[4] + checkCode[1]
 * packet header: 0xAA 0x55
 * packet 
 */
static void CoMcu_HandlePacket(void) {
    uint8_t rxBuf[CO_MCU_PKT_SIZE_MAX] = {0};
    memcpy(rxBuf, CoMcu_rxBuf, CO_MCU_RX_BUF_SIZE);   // 尽快释放 CoMcu_rxBuf

    if (CO_MCU_HEADER_MAGIC1 != rxBuf[CO_MCU_PKT_HEADER_START] ||
        CO_MCU_HEADER_MAGIC2 != rxBuf[CO_MCU_PKT_HEADER_START + 1]) {
        return;
    }

    uint8_t cmd          = rxBuf[CO_MCU_PKT_CMD_START];
    uint8_t dataLen      = rxBuf[CO_MCU_PKT_LENGTH_START];
    uint8_t srcComponent = rxBuf[CO_MCU_PKT_SRC_COMPONENT_START];

    if (!CoMcu_VerifyCheckCode(rxBuf, dataLen)) {
        return;
    }

    uint16_t distance = 0;

    switch (cmd) {
        case CO_MCU_CMD_FORWARD:
            distance = (rxBuf[CO_MCU_PKT_DATA_START + 1] << 8) | rxBuf[CO_MCU_PKT_DATA_START + 2];
            if (true == Posture_Forward(distance, rxBuf[CO_MCU_PKT_DATA_START + 3])) {
                CoMcu_AckResult(CO_MCU_CMD_FORWARD, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_FORWARD, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_BACKWARD:
            distance = (rxBuf[CO_MCU_PKT_DATA_START + 1] << 8) | rxBuf[CO_MCU_PKT_DATA_START + 2];
            if (true == Posture_Backward(distance, rxBuf[CO_MCU_PKT_DATA_START + 3])) {
                CoMcu_AckResult(CO_MCU_CMD_BACKWARD, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_BACKWARD, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_TURN_LEFT:
            if (true == Posture_TurnLeft(rxBuf[CO_MCU_PKT_DATA_START + 1], rxBuf[CO_MCU_PKT_DATA_START + 2])) {
                CoMcu_AckResult(CO_MCU_CMD_TURN_LEFT, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_TURN_LEFT, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_TURN_RIGHT:
            if (true == Posture_TurnRight(rxBuf[CO_MCU_PKT_DATA_START + 1], rxBuf[CO_MCU_PKT_DATA_START + 2])) {
                CoMcu_AckResult(CO_MCU_CMD_TURN_RIGHT, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_TURN_RIGHT, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_STOP:
            if (true == Posture_Stop()) {
                CoMcu_AckResult(CO_MCU_CMD_STOP, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_STOP, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_TERMINATE:
            if (true == Posture_Terminate()) {
                CoMcu_AckResult(CO_MCU_CMD_TERMINATE, srcComponent, true);
            } else {
                CoMcu_AckResult(CO_MCU_CMD_TERMINATE, srcComponent, false);
            }
            break;
        case CO_MCU_CMD_RESULT_ACK:
        default:
            break;
    }
}

void CoMcu_AckResult(uint8_t cmd, uint8_t srcComponent, bool success) {
    memset(CoMcu_txBuf, 0, sizeof(CoMcu_txBuf));
    CoMcu_txBuf[0] = CO_MCU_HEADER_MAGIC1;
    CoMcu_txBuf[1] = CO_MCU_HEADER_MAGIC2;
    CoMcu_txBuf[2] = CO_MCU_CMD_RESULT_ACK;
    CoMcu_txBuf[3] = CO_MCU_PKT_SRC_COMPONENT_SIZE + CO_MCU_PKT_CMD_SIZE + 1;
    CoMcu_txBuf[4] = srcComponent;
    CoMcu_txBuf[5] = cmd;
    CoMcu_txBuf[6] = success ? 0 : 1;
    CoMcu_CalcCheckCode(CoMcu_txBuf[3]);

    HAL_UART_Transmit(&huart1, CoMcu_txBuf, CO_MCU_TX_BUF_SIZE, HAL_MAX_DELAY);
}

void CoMcu_AckProcess(uint8_t cmd, uint8_t srcComponent, uint8_t* data, uint8_t dataLength) {
    memset(CoMcu_txBuf, 0, sizeof(CoMcu_txBuf));
    CoMcu_txBuf[0] = CO_MCU_HEADER_MAGIC1;
    CoMcu_txBuf[1] = CO_MCU_HEADER_MAGIC2;
    CoMcu_txBuf[2] = CO_MCU_CMD_PROCESS_ACK;
    CoMcu_txBuf[3] = dataLength + CO_MCU_PKT_CMD_SIZE + CO_MCU_PKT_SRC_COMPONENT_SIZE;
    CoMcu_txBuf[4] = srcComponent;
    CoMcu_txBuf[5] = cmd;
    memcpy(&CoMcu_txBuf[6], data, dataLength);
    CoMcu_CalcCheckCode(CoMcu_txBuf[3]);

    HAL_UART_Transmit(&huart1, CoMcu_txBuf, CO_MCU_TX_BUF_SIZE, HAL_MAX_DELAY);
}

void CoMcu_AckQuery(uint8_t cmd, uint8_t srcComponent, uint8_t* data, uint8_t dataLength) {
    // TODO
}

/* Start to receive mutiple types from UART1 */
void CoMcu_StartRx(void) {
    coMcu_uart1RxSemaphore = xSemaphoreCreateBinary();

    // 启动 DMA 接收（只启动一次）
    // __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
    // 先接收 Header[2] + cmd[1] + length[1]，再根据length进行第二次中断，减少中断次数
    HAL_UART_Receive_DMA(&huart1, CoMcu_rxBuf, CO_MCU_PKT_DATA_START);
    // HAL_UARTEx_ReceiveToIdle_DMA(&huart1, CoMcu_rxBuf, CO_MCU_RX_BUF_SIZE);
}

void CoMcu_HandleRx(void) {
    if (xSemaphoreTake(coMcu_uart1RxSemaphore, portMAX_DELAY) == pdTRUE) {
        // process packet from ESP32
        CoMcu_HandlePacket();

        // 重新启动DMA接收（从缓冲区起始位置开始）
        // HAL_UART_DMAStop(&huart1);
        // HAL_UARTEx_ReceiveToIdle_DMA(&huart1, CoMcu_rxBuf, CO_MCU_PKT_DATA_START);
        HAL_UART_Receive_DMA(&huart1, CoMcu_rxBuf, CO_MCU_PKT_DATA_START);
    }
}

BaseType_t CoMcu_HandleRxIT(void) {
    static bool isHeaderReceived = false;
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    if (isHeaderReceived == false) {
        isHeaderReceived = true;
        uint8_t length = CoMcu_rxBuf[CO_MCU_PKT_LENGTH_START] + 1;
        HAL_UART_Receive_DMA(&huart1, &CoMcu_rxBuf[CO_MCU_PKT_DATA_START], length);
    } else {
        isHeaderReceived = false;
        xSemaphoreGiveFromISR(coMcu_uart1RxSemaphore, &xHigherPriorityTaskWoken);
    }

    return xHigherPriorityTaskWoken;
}
